int freeRam () {
  extern int __heap_start, *__brkval;
  int v;
  return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval);
}

void printIntHex(char *s, int v) { 
  Serial.print(s);
  Serial.print(": ");
  Serial.println(v, HEX);
}

void printRam() {
  extern int __heap_start, __bss_start, __data_start, *__brkval;
  int stack;
  int heapend;

  if((int)__brkval == 0) {
    heapend = (int)&__heap_start;
  } 
  else {
    heapend = (int) __brkval;
  }

  printIntHex("End of RAM:   ", RAMEND);
  printIntHex("Stack pointer:", SP);
  printIntHex("Stack pointer:", (int) &stack);
  printIntHex("Heap end:     ", heapend);
  printIntHex("Heap start:   ", (int) &__heap_start);
  printIntHex("BSS start     ",   (int) & __bss_start);
  printIntHex("Data start    ",   (int) & __data_start);

}

int d2u(int d) {
  return MIN_PULSE_WIDTH + int(float(d) * servo_scale);
}

int u2d(int u) {
  return int(float(u - MIN_PULSE_WIDTH) / servo_scale + 0.5);
}
